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ABSTRACT 

We  present  an  C>(n^)  algorithm  for  planning  a  coordinated 
collision-free  motion  of  two  independent  robot  systems  of  cer- 
tain kinds,  each  having  two  degrees  of  freedom,  >vhich  move  in 
the  plane  amidst  polygonal  obstacles  having  a  toLal  of  n  corners. 
We  exemplify  our  technique  in  the  case  of  two  "planar  Stanford 
arms",  but  also  discuss  the  case  of  two  discs  or  convex  translat- 
ing objects.  The  algorithm  improves  previous  algorithms  for  this 
kind  of  problems,  and  can  be  extended  to  a  fairly  simple  general 
technique  for  obtaining  efficient  coordinated  motion  planning 
algorithms. 


1.  Introduction 

In  this  paper  we  present  a  new  approach  to  the  design  of  efficient  algo- 
rithms for  coordinated  collision-free  motion  planning  for  two  independent 
robot  systems  moving  amidst  a  collection  of  obstacles  which  they  must  avoid 
(and  also  avoid  collision  into  one  another).  We  exemplify  our  techniques  in 
the  case  of  two  "planar  Stanford  arms",  where  each  arm  is  modeled  as  a  line 
segment  which  can  slide  forward  and  backward  through  some  fixed  point, 
and  can  also  rotate  about  that  point;  thus  each  arm  has  two  degrees  of  free- 
dom (see  Fig.  1).  The  arms  are  assumed  to  be  moving  in  the  plane  amidst  a 
collection  of  polygonal  obstacles,  having  a  total  of  n  corners.  This  problem 
has  been  studied  by  Fortune,  Wilfong  and  Yap  [FWY],  who  present  an 
O(n^)  algorithm  for  it.  We  present  here  an  improved  algorithm  whose  com- 
plexity is  only  0{n^).  This  algorithm  is  a  special  case  of  a  more  general  tech- 
nique that  we  introduce  here,  and  which  can  be  applied  to  obtain  efficient 
algorithms  for  various  other  coordinated  motion  planning  problems.  For 
example,  we  obtain  an  0{n^)  algorithm  for  coordinating  the  motion  of  two 
independent  discs  (or  more  generally  of  two  translating  convex  objects)  in  the 
plane,  amidst  polygonal  obstacles  as  above,  thereby  matching  and  extending  a 
previous  result  of  Yap  [Yal],  as  a  special  case  of  our  more  general  approach. 
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Another  recent  work  on  coordinated  motion  planning  is  [EL];  they  use  a 
more  heuristic  approach,  whose  worst  case  performance  is  inferior  to  that  of 
our  technique.  We  also  obtain  nearly  quadratic  algorithms  for  coordinated 
motion  planning  for  practically  any  pair  of  independently  moving  systems 
with  two  degrees  of  freedom  each,  and  several  other  efficient  coordinated 
motion  planning  algorithms  for  pairs  of  more  complex  systems,  or  even  of 
larger  groups  of  such  systems. 

As  in  many  standard  motion  planning  algorithms,  oux  goal  is  to  obtain  a 
discrete  combinatorial  representation  of  the  space  FP  of  all  free  placements 
of  our  system,  and  decompose  that  space  into  its  (arcwise)  connected  com- 
ponents, so  that,  given  an  initial  and  final  placements  of  the  system,  we  can 
check  whether  they  both  lie  in  the  same  connected  component  of  FP  (which  is 
exactly  the  condition  for  a  continuous  collision-free  motion  to  exist  between 
these  two  placements),  and  if  so  we  can  also  p^an  such  a  motion.  See  [SSI], 
[SS2],  [SS3],  [OSYl],  [OSY2],  [LS],  [KS],  [SiS],  [Yal],  [FWY],  [KO]  for  a 
sample  of  recent  algorithms  of  this  kind. 

One  of  the  standard  approaches,  initially  proposed  in  [SSI],  [SS2],  and 
used  in  many  recent  papers,  is  to  obtain  a  cell  decomposition  of  FP  into  sim- 
ple and  connected  cells,  determine  adjacency  of  pairs  of  these  cells,  and 
represent  this  structiire  by  a  connectivity  graph,  whose  vertices  are  these  cells, 
and  whose  edges  connect  pairs  of  adjacent  cells.  Then  any  instance  of  motion 
planning  between  a  specified  pair  of  an  initial  and  final  placements  of  our 
system  can  be  easily  reduced  to  a  purely  combinatorial  path-searching  prob- 
lem in  the  connectivity  graph. 

We  can  exemplify  the  cell  decomposition  approach  in  the  case  of  coordi- 
nated motion  planning  for  two  independent  systems  B-^,  B2,  each  having  two 
degrees  of  freedom,  and  both  moving  in  a  2-D  polygonal  space  whose  boun- 
dary has  n  comers.  This  will  motivate  our  approach,  and  will  also  serve  to 
introduce  some  terminology.  We  note  that  FP  is  four-dimensional,  and  we 
decompose  it  by  first  calculating  FP{Bi),  which  is  the  (two-dimensional) 
space  of  all  free  placements  of  flj,  ignoring  the  presence  of  ^2-  F^or  ^^^ 
placement  X€FP(fli),  let  us  denote  by  FP^C^i)  the  two-dimensional  "fiber" 
space  of  all  free  placements  of  Bi,  when  Bi  is  fixed  at  placement  X  (i.e.  it  is 
regarded  as  another  fixed  obstacle).  Now  if  we  vary  X  continuously  within 
FP{Bi),  the  combinatorial  representation  of  FPxiBi)  will  remain  the  same, 
unless  X  crosses  one  of  finitely  many  critical  curves,  at  which  the  structure  of 
FPxiBi)  changes  discontinuously.  In  the  case  where  Bi  and  B2  are  two 
planar  Stanford  arms,  it  is  shown  in  [FWY]  that  the  number  of  critical  curves 
is  0(n),  that  they  are  all  algebraic  of  some  fixed  (and  small)  maximal  degree, 
and  that  they  thus  partition  FP{Bi)  into  0(n^)  non-critical  regions,  namely 
the  connected  components  (i.e.  faces)  of  the  arrangement  of  these  critical 
curves.  It  is  further  shown  in  [FWY]  that  the  combinatorial  complexity  of 
each  fiber  space  FPxiBj)  is  0(n),  so  that,  in  particular,  it  consists  of  0{n) 
connected  components  Ci{X),C2{X),  .  .  .  ,  with  the  property  that  the  com- 
binatorial representation  of  each  component  remains  the  same  as  X  varies 
within  a  non-critical  region.  Thus,  for  each  non-critical  region  R  of  FP(Bi), 
we  obtain  0{n)  simple  connected  cells  within  FP,  all  of  which  project  onto  R, 
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and  each  having  the  form  {(,X,Y)  |  XiR,  YiCj{X)}  for  some  ;.  The  collec- 
tion of  all  these  cells,  over  all  non-critical  regions  R,  yields  the  desired  cell 
decomposition  of  FP.  The  same  approach  has  also  been  used  in  [SS3]  to 
obtain  an  algorithm  for  planning  the  coordinated  motion  of  two  independent 
discs. 

Unfortimately,  the  total  combinatorial  complexity  of  all  the  resulting 
cells  is  C>(n^)  (both  for  the  case  of  two  Stanford  arms  and  for  the  case  of  two 
discs).  That  this  is  far  from  being  optimal  has  been  demonstrated  by  Yap 
[Yal]  for  the  case  of  two  discs,  by  giving  an  0{n^)  (specially-tailored)  algo- 
rithm for  this  case. 

In  this  paper  we  introduce  a  new,  and  surprisingly  simple,  approach  to 
cell  decomposition  of  FP  for  two  independently  moving  objects,  and  use  it  to 
obtain  an  improved  0(n^)  algorithm  for  the  case  of  two  Stanford  arms.  As  a 
matter  of  fact,  our  technique  is  fairly  general  and  can  be  applied  to  many 
coordinated  motion  planning  problems  involving  two  independently  moving 
systems.  For  example,  for  the  general  case  of  two  independent  systems,  each 
with  two  degrees  of  freedom,  moving  in  a  2-D  polygonal  space  with  n 
comers  (assuming  that  the  complexity  of  each  moving  system  is  small  and 
fixed),  we  obtain  a  nearly  quadratic  algorithm  (see  below  for  a  more  precise 
formulation).  Efficient  algorithms  for  various  other  special  types  of  system 
are  discussed  in  the  concluding  section  6. 

2.  Overview  of  our  coordinated  motion  planning  technique 

Here  is  a  description  of  our  approach.  Suppose  81,82  are  two  systems 
with  two  degrees  of  freedom  each.  We  first  calculate  FP{8i)  and  FP{82) 
(defined  in  an  analogous  manner),  and  decompose  each  of  these  spaces  into  a 
collection  of  (pairwise  openly  disjoint)  simple  connected  cells,  such  that  each 
cell  is  bounded  by  a  constant  number  of  arcs  of  small  and  fixed  maximal  alge- 
braic degree;  let  {cl,  .  .  .  ,clj  and  {c? ,  .  .  .  ,c^ J  be  the  resulting  decompo- 
sitions for  FP{Bi)  and  FP{82)  respectively.  (Each  of  these  calculations  is  a 
motion  planning  problem  for  a  system  having  only  two  degrees  of  freedom, 
and  can  be  solved  efficiently  either  by  a  general  procedure  presented  in  [GSS] 
(and  reviewed  below),  or  by  other  efficient  ad-hoc  techniques;  see  below  for 
additional  discussion  of  this  issue.)  We  also  assume  that  the  number  of  adja- 
cent pairs  of  cells  {c},c}')  (resp.  {cj,cj'))  is  only  0{ni)  (resp.  0(/i2)).  This 
is  a  natural  assumption  for  2-D  problems,  and  follows  generally  from  Euler's 
formula. 

Next,  for  each  l</<ni  and  l<;<n2,  let  FP,,  denote  the  intersection  of 
FP  with  the  Cartesian  product  cj  x  cj.  Since  both  cj  and  cj  are  bounded  by 
0(1)  curves,  each  of  which  describes  either  a  contact  of  5i  or  of  82  with 
some  fixed  obstacle  feature,  or  else  is  an  artificial  cell  boundary,  we  argue 
that  FP,j  can  be  regarded  as  the  free  configuration  space  of  the  combined  sys- 
tem 8 1,  82,  when  moving  in  the  presence  of  only  0(1)  obstacles,  or,  more 
precisely,  subject  to  only  0(1)  collision-constraints.  Thus,  assuming  that  81 
and  8 2  have  both  0(1)  combinatorial  complexity,  it  follows  that  the  combina- 
torial complexity  of  each  FP,j  is  constant,  and  that  a  decomposition  of  it  into 
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0(1)  4-D  cells  (e.g.  of  the  form  mentioned  above),  together  with  determina- 
tion of  the  adjacency  of  these  cells  (within  FPij)  can  be  obtained  in  constant 
time.  (At  worst  such  a  decomposition  can  be  obtained  using  general  (but 
inefficient)  techniques  such  as  in  [SS2],  [Ca];  alternatively,  more  efficient  ad- 
hoc  decomposition  techniques  are  preferred;  they  will  improve  the  constant 
size  of  each  FPy,  but  will  not  change  the  overall  asymptotic  complexity  of  the 
algorithm.) 

Next,  we  "glue"  all  these  pieces  of  FP  together;  this  amounts  to  deter- 
mining which  cells  within  one  such  piece  FP,j  are  adjacent  to  which  cells  in 
some  other  "neighboring"  piece  FPfj- .  This  is  also  a  fairly  simple  task,  which 
proceeds  as  follows.  We  argue  that  for  a  cell  Q  within  some  FPij  to  be  adja- 
cent to  a  cell  C  within  another  FPfy  it  is  necessary  that  either  i=i'  and  c?, 
c}  are  adjacent  in  FP(B 2),  or  that  j=j'  and  cj,  c}  are  adjacent  in  FP{B^. 
Thus  there  are  only  0(^1/12)  pairs  of  pieces  of  FP  that  have  to  be  glued,  and 
the  gluing  of  each  such  pair  can  be  accompUshed  in  constant  time,  since  it 
involves  a  pair  of  pieces,  each  having  0(1)  many  cells  of  a  constant  total 
complexity  (so  that  we  can  tell  in  0(1)  time  which  pairs  of  these  cells  are 
adjacent). 

We  thus  obtain  a  decomposition  of  FP  into  0(nin2)  simple  connected 
cells,  such  that  the  number  of  adjacent  pairs  of  these  cells  in  FP  is  also 
0(nin2).  This  enables  us  to  represent  FP  by  a  connectivity  graph  CG  (as 
above)  whose  size  is  0{n-^n2),  and  then  to  use  CG  to  reduce  any  motion 
planning  instance  to  path  searching  through  that  graph.  This  completes  our 
general  solution  of  the  coordinated  motion  planning  problem  at  hand.  We 
remark  that  our  technique  is  closely  related  to  a  technique  recently  proposed 
in  [SS4]  concerning  motion  planning  in  environments  with  "boimded  local 
complexity". 

2.1.  Applications  to  special  cases  and  some  technical  details 

The  quantities  n^,  rii  defined  above  are  both  0{n)  for  the  case  of  Stan- 
ford arms,  and  for  the  case  of  discs,  or,  more  generally,  for  the  case  of  a  pair 
of  convex  polygons  (bounded  by  a  constant  number  of  edges)  translating 
independently  in  the  plane.  For  the  case  of  Stanford  arms,  this  was  shown  in 
[FWY]  and  in  [SiS].  The  claim  for  discs  or  for  translating  convex  polygons 
follows  from  the  results  of  [KLPS].  Moreover,  in  both  cases  one  can  calculate 
the  spaces  FPiB{),  FPifii)  in  subquadratic  (actually  close  to  linear)  time, 
which  is  thus  dominated  by  the  quadratic  cost  of  constructing  the  "product" 
space  FP.  We  thus  obtain  Oin^)  algorithms  for  all  these  problems. 

For  general  systems  with  two  degrees  of  freedom,  n^  and  ni  might  be 
quadratic  in  n,  if  we  calculate  the  entire  2-D  spaces  FP{Bi),  FPiBi).  How- 
ever, extending  a  basic  observation  in  [PSS]  and  [GSS],  there  is  no  need  to 
calculate  these  spaces  in  their  entirety;  it  suffices  to  obtain  the  connected 
component  C;  of  FP{B,)  which  contains  the  initial  placement  Zi  of  Bt,  for 
/  =  1,2,  because  any  free  placement  of,  say  B 1 ,  which  is  reachable  from  Zi  by 
a  continuous  coUision-free  motion  (whether  with  B2  present  or  not)  must  lie 
in  Ci,  and  similarly  for  ^2-  Thus  it  suffices  to  apply  the  cell  decomposition 


method  described  above  to  the  two  components  C^,  C2  rather  than  to  the 
entire  spaces  FP{Bi),  FP{B2)-  But,  as  shown  in  [GSS],  the  combinatorial 
complexity  of  a  single  component  C  of  the  free  configuration  space  of  a  sys- 
tem B  having  two  degrees  of  freedom,  is  0(kj{n)),  where  n  is  the  number  of 
geometric  constraints  on  the  free  placements  of  B  (all  assumed  to  be 
representable  by  algebraic  curves  of  low  and  constant  maximal  degree),  j  is  a 
fixed  parameter  depending  on  the  degree  of  these  constraints  (more  pre- 
cisely, it  is  2  +  the  maximum  number  of  intersections  between  any  pair  of 
these  curves),  and  \,{n)  is  the  maximum  length  of  {n,s)  Davenport-Schinzel 
sequences,  which  is  nearly  linear  in  n  for  any  fixed  s  (see  [HS],  [ASS]  for 
more  detail).  Moreover,  such  a  component  can  be  calculated  (under  an 
appropriate  model  of  computation)  in  time  0(\j{n)  log^n)  [GSS].  Hence, 
applying  these  results  to  our  case,  we  conclude  that  both  n-i  and  ni  can  be 
taken  to  be  nearly  linear  in  n,  and  that  the  relevant  portion  of  FP  within 
Ci  >.  C2  (which  includes  aJl  placements  of  the  combmed  system  reachable 
from  (Zi,Z2))  has  only  0(X,j(ni)X,,(n2))  combinatorial  complexity  (for 
appropriate  parameters  ^1,  ^'2).  and  can  be  calculated  within  a  similar  amount 
of  time  (which  also  dominates  the  time  needed  for  the  initial  calculation  of 
Ci,C2). 

In  the  following  sections  of  the  paper  we  provide  details  about  the  calcu- 
lation of  each  FPy.  Even  though  this  takes  only  constant  time  per  "piece", 
one  should  aim  to  make  this  time  as  small  as  possible,  or  else  the  constant  of 
proportionahty  in  the  time  bound  for  the  algorithm  may  be  unacceptably 
high.  For  those  who  really  scorn  any  such  consideration,  we  can  recommend 
the  very  general  Collins'  cell  decomposition  technique,  as  used  in  [SS2], 
which  runs  in  time  doubly-exponential  in  the  number  of  degrees  of  freedom 
(which  is  nevertheless  constant  in  our  case),  or  the  improved  general  pro- 
cedure of  Canny  [Ca],  which  has  only  singly-exponential  time  complexity. 
However,  we  will  illustrate  in  the  following  sections  more  explicit  and  effi- 
cient decomposition  techniques  for  the  case  of  two  Stanford  arms  and  for  the 
case  of  two  discs. 

The  remainder  of  this  paper  is  organized  as  follows.  In  section  3  we 
analyze  the  free  configuration  space  of  a  single  planar  Stanford  arm,  as  a 
preparatory  step  to  the  motion  coordination  algorithm,  which  is  presented  in 
section  4.  Section  5  discusses  the  case  of  two  independent  discs  (or  translating 
convex  objects),  and  section  6  contains  concluding  remarks,  including  various 
extensions  of  our  techniques.  Some  technical  details  about  the  algebraic  struc- 
ture of  critical  curves  arising  in  the  case  of  two  Stanford  arms  are  given  in 
Appendix  A. 

3.  The  free  configuration  space  of  one  planar  Stanford  arm 

We  now  exemplify  the  general  technique  outlined  above  in  the  special 
case  of  two  independent  planar  Stanford  arms.  For  this  we  first  analyze  the 
case  of  a  single  arm.  A  single  planar  Stanford  arm  A  is  modeled  as  a  Ime  seg- 
ment PQ  which  is  constrained  to  touch  a  fixed  point  C,  and  thus  has  two 
degrees  of  freedom  -  rotation  about  C  and  translation  along  its  length 
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(through  C).  The  workspace  D  of  A  is  the  disc  centered  at  C  and  having 
radius  \PQ  |,  and  the  circle  surrounding  D  is  denoted  by  E.  D  is  assumed  to 
contain  polygonal  obstacles,  each  of  which  is  bounded  by  a  closed  simple 
polygonal  curve  composed  of  straight  walls  (edges)  and  of  corners  (vertices) , 
such  that  any  pair  of  obstacles  has  disjoint  interiors,  and  the  total  number  of 
their  comers  is  n.  Each  placement  of  A  at  which  P  does  not  lie  at  C  (including 
non-free  placements)  can  be  uniquely  represented  by  the  placement  in  D  of 
the  tip  P  of  A.  For  simplicity  of  exposition  (but  with  no  real  loss  of  general- 
ity), we  exclude  placements  at  which  P  coincides  with  C,  so  D—{C}  is  a 
representation  of  the  space  of  placements  of  A.  (Our  analysis  can  be  easily 
adapted  to  cover  cases  in  which  there  are  some  physical  limits  on  the  possible 
motions  of  A,  e.g.  when  it  can  slide  only  up  to  certain  length  or  rotate  only 
within  some  restricted  range  of  orientations,  etc.)  The  free  configuration 
space  FP  (A)  of  A  contains  placements  at  which  A  does  not  touch  any  obstacle 
or  (again  for  exposition  sake)  any  point  of  the  circle  E.  Let  BFP{A)  be  the 
boundary  of  FP{A).  It  is  shown  in  [P*VY]  (and  also  derived  in  Lemma  4.1  of 
[SiS])  that  the  number  of  vertices  of  BFP{A)  is  C>(n),  and  that  BFP{A)  con- 
sists of  0{n)  simple  arcs,  each  of  which  is  either  a  line  segment  (a  portion  of 
a  wall  or  of  a  radius  emerging  from  C),  an  arc  of  £,  or  a  portion  of  some 
conchoid  of  Nicomedes,  which  is  a  fourth-degree  algebraic  curve  obtained  as 
the  locus  of  the  endpoint  P  of  the  segment  A  as  its  other  endpoint  Q  moves 
along  a  straight  wall  while  it  keeps  touching  the  point  C  (see  [Lo],[SSl]  and 
the  appendix  for  more  detail). 


Figure  1  :  A  portion  of  FP{A) 


It  is  easily  verified  that  each  ray  emerging  from  C  intersects  FP{A) 
either  in  an  open  line  segment  or  in  the  empty  set.  Using  this  property,  we 
can  apply  to  FP{A)  the  following  "angular"  decomposition.  For  each  vertex 
viBFP{A)  (other  than  C)  draw  a  ray  Ly  which  emerges  from  C,  passes 
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through  V  and  yields  the  line  segment  FP(A)nLy.  The  set  of  these  rays  yields 

an  angular  decomposition  FP{A)  =   U  C/  into  m  =  0{n)  cells,  so  that  each 

such  cell  C/  is  bounded  by  two  radial  segments  (contained  in  two  adjacent  del- 
imiting rays,  and  one  or  both  of  which  may  degenerate  to  a  single  point),  a 
"ceiling"  which  is  either  a  line  segment  (along  a  wall)  or  a  circular  arc  (of  E), 
and  a  "floor"  which  is  either  an  arc  of  a  conchoid  of  Nicomedes  (generated  by 
a  wall  touched  by  Q),  or  the  single  point  C  (see  Fig.  1).  (For  convenience,  we 
will  assume  that  each  c/  contains  its  two  bounding  radial  segments.) 

In  the  sequel  we  will  make  use  of  the  notion  of  the  radial  shift  map 
rj:^:D—{C}  ^D-{C},  introduced  in  [SiS],  which  is  defined  so  that  for  each 
X^D  -{C}  its  image  r^iX)  is  the  point  occupied  by  Q  when  the  arm  A=PQ  is 
placed  with  P  at  X  (and  with  C  touching  PQ).  It  is  clear  (see  also  [SiS])  that 
r^  maps  a  segment  of  a  radius  of  D  onto  a  similar  segment,  that  it  is  unde- 
fined at  A  -  C,  ivnJ  that  it  maps  a  line  segment  L  onto  an  arc  of  a  conchoid 
of  Nicomedes  (defined  by  C  and  L) ,  and  vice  versa. 

In  order  to  handle  below  the  case  of  two  Stanford  arms,  we  next  analyze 
the  effect  on  FP(A)  of  the  insertion  of  a  new  wall  edge  W  into  the  workspace 
D  (such  that  W  does  not  pass  through  C).  W  induces  a  modified  smaller  free 
configuration  space,  which  we  denote  by  FP^{A),  and  which  can  be  written 

m 

as    U  FPw{ci),  where  FP^ici)  is   the  restriction  of  FPjv(A)   to  the  cell 

Ci,  J  =  1,  .  .  .  ,m.  For  each  fixed  cell  c  from  the  above  angular  decomposition, 
FPwic)  is  the  difference  c  -FORB^{c),  where  FORBw{c)  is  the  set  of  forbid- 
den (i.e.  non-free)  placements  of  A  within  c  (the  closure  of  c)  at  which  A 
intersects  W.  FORB^,{c)  can  be  easily  obtained  as  follows.  First_ define  the 
"shadow"  of  W  with  respect  to  C  (and  to  D)  as  the  set  SH(W)CD  consisting 
of  points  X  for  which  there  exists  a  point  YiW  which  belongs  to  the  closed 
segment  XC.  Next,  let  Fi  =  SH(W)nc  and  let  Fj  =  5i/(W)nr^(^.  Then 
FORBwic)  =  FiUr^(F2)  (see  Fig.  2  for  an  example  of  FORBw{c)). 

It  is  easily  seen  that  the  boundary  of  SH(W)  for  a  line  segment  W  consists  of 
(the  portion  within  D  of)  W,  of  the  two  radial  extensions  from  its  endpoints 
to  the  boundary  E  of  D,  and  the  (smaller)  circular  portion  of  E  between  the 
ends  of  these  extensions. 

We  end  this  section  by  mentioning  two  additional  properties  of  FPw{c). 

(PI)  FFvv'(c)  intersects  the  two  radial  edges  of  c  in  two  respective  sets,  each 
of  which  is  either  a  line  segment  or  empty. 

(P2)  Let  ^  be  an  arc  from  the  boundary  of  cUrx(c).  Then  SH{W)ny  is 
either  a  subarc  of  -y,  or  a  single  point  of  tangency  between  y  and 
SH(W),  or  contains  a  finite  number  of  intersection  points,  each  of 
which  is  a  transversal  crossing  of  -y  into  SH(W).  This  claim  actually 
states  that  if  7  is  tangent  to  SH(W)  then  they  have  a  single  point  in 
common.  Since  SH{W)  is  convex,  the  claim  is  obvious  when  7  is  a  line 
segment  or  a  circular  arc  (of  E).  If  7  is  an  arc  of  a  conchoid  of 
Nicomedes  which  is  tangent  to  SH{W),  then  the  claim  follows  from  the 
fact  that  a  simple  closed  arc  of  such  a  conchoid  bounds  a  convex 
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domain  (see  [Lo]),  and  that  this  domain  is  linearly  separable  from 
SH(W)  by  their  common  tangent,  as  easily  follows  from  definition. 


c  is  the  domain 
bounded  by  K. L.M.N. 


Figure  2  :  An  example  of  FORBt/^t^c) 


4.  The  case  of  two  independent  planar  Stanford  arms 

We  next  consider  the  case  of  the  coordinated  motion  of  two  planar  Stan- 
ford arms  Ai  and  Ai  modeled  as  two  line  segments  PiQi  and  PiQi,  which 
are  free  to  move  in  the  plane  but  are  constrained  to  touch  two  fixed  points 
Ci  and  Ci,  respectively,  not  to  intersect  one  another,  and  to  avoid  a  collec- 
tion of  polygonal  obstacles  with  a  total  of  n  comers,  as  above  (see  Fig.  3). 
The  workspace  D  of  the  arms  is  the  union  of  two  discs  D  i  ,D  2  centered  at 
CiXi  and  having  radii  1/^1211,1^222  1  respectively.  We  denote  the  circle 
surrounding  D\  (resp.  D2)  by  E^  (resp.  Ei).  Each  arm  A/  has  two  degrees 
of  freedom  which  can  be  uniquely  represented  (under  the  restrictions  made 
above)  by  the  location  of  the  arm's  tip  P,,  /  =  1,2.  Each  arm  Ai  induces  its 
own  "single-arm"  free  configuration  space  FP{Ai),  i  =  l,2,  in  which  we 
assume  the  other  arm  A 3./  to  be  absent.  These  (2-D)  spaces  have  the  struc- 
ture described  in  the  preceding  section. 

Let  us  denote  by  FP=FP{Ai,A2)  the  (four-dimensional)  free  configura- 
tion space  for  both  arms  Ai,A2,  and  its  boundary  by  BFP{Ai,A2)-  Each 
placement  in  FP(Ai,A2)  can  be  represented  as  a  point  of  D1XD2C/?  (more 
precisely  of  (£>i-{Ci})x(D2-{C2})).  To  simplify  the  presentation  of  our 
algorithm,  we  assume  that  the  obstacles  lie  in  general  position  (with  respect 
to  the  two  arms)  which  means: 

(i)    There  is  no  collinear  triplet  {Xi,X2,X-i},  where  each  X,  is  either  a  vertex 
of  BFP(Ai)U5F/'(A2)  or  one  of  Ci,C2. 


(ii)  For  J  =  l,2,  there  is  no  collinear  triplet  {Xi,X2,X3},  with  Xi€{Ci,C2}, 
Xi  a  vertex  of  FP(A,),  and  X3  the  image  of  a  vertex  of  FP{A2-i)  under 
the  radial  shift  r^^_^  (the  reason  for  this  somewhat  peculiar  condition  is 
clarified  later). 


Figure  3:  Two  planar  Stanford  arms 


c! 


FP{Ai,A2)    can    be    described    as    follows.    Let    c\. 


.«^n, 


and 


,  .  ,c„j  be  the  cells  which  form  the  angular  decomposition  of  FP{Ai) 
and  FP{A2),  respectively;  then  a  convenient  decomposition  of  FP{Ai,A2)  is 

(*)  U^U  JP{AuA2)\ic}.cj) 

where  the  subspace  FP{Ai,A2)\(c}xc-)  (which  we  denote  shortly  as  FPij)  is 
the  restriction  of  FP{Ai,A2)  to  the  four -dimensional  Cartesian  product 
clxcj,  l<i<ni,  l<y<«2-  Following  the  general  strategy  outlined  in  the 
introduction,  we  next  apply  a  cell-decomposition  technique  to  each  subspace 
FPij  in  the  decomposition  (*).  Clearly  each  FPy  has  fixed  complexity, 
because  the  coordinated  motion  of  A 1  and  A  2  withm  the  two  corresponding 
cells  c},  cj  is  constrained  by  at  most  four  walls,  by  two  constraints  on  the 
range  of  allowed  orientation  of  each  arm,  and  by  the  constraint  that  the  two 
arms  avoid  coUision  with  one  another.  In  what  follows  we  provide  more 
technical  detail  on  how  to  obtain  this  (constant-size)  decomposition  in  a  con- 
venient and  economic  manner;  we  note  that  a  more  global  but  closely  related 
decomposition  of  FP  has  also  been  given  in  [FWY] . 

Let  BD{c^)  denote  the  circular  sequence  of  arcs  and  vertices  that  consti- 
tute the  boundary  of  c'^^c'j.  BD{ry^,{c^))  denotes  the  corresponding 
sequence  for  the  boundary  of  the  radiaj-shift  image  r^,  of  c^.  For  each 
X^c^=c}  let  Ai{X)  denote  the  corresponding  placement  of  Ai.  Ai{X)  can  be 
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regarded 

as  a  new  wall  edge  for  the  second  arm  A 2  to  avoid,  and  yields  a  constrained 
free  space  FPx{c^)  (a  shorthand  notation  for  F/'ai(x)(c^));  the  structure  of 
FPx{c^)  has  been  described  in  the  preceding  section.  It  is  easy  to  check  from 
the  geometry  of  the  ceU  c^  that  FPx{c^)  consists  of  at  most  two  connected 
components,  each  of  which  is  a  simply  connected  subregion  of  c^  (Fig.  4 
illustrates  an  example  of  two  components). 


A  component 
of   FPx(c2) 


Figure  4  :  An  example  of  FPxic^)  with  two  components 

Each  of  these  components  can  be  represented  in  a  purely  discrete  manner, 
which  does  not  depend  continuously  on  X,  by  the  circular  sequence  of  the 
arcs  appearing  along  its  boundary  (say  in  counterclockwise  order),  each  of 
which  is  either  one  of  the  four  arcs  bounding  c  ^ ,  or  (a  portion  of)  A 1  (X) ,  or 
a  portion  of  the  radial-shift  image  r^  ^  of  A 1  (X)  (which  is  a  conchoidal  arc) , 
or  a  portion  of  the  radial  edge  emanating  from  an  endpoint  of  A 1  (X)  (if  this 
endpoint  lies  inside  c^),  or  the  radial-shift  image  of  such  a  radial  edge  (if  the 
corresponding  endpoint  of  Ai(X)  lies  inside  r4,(c^)).  See  Fig,  5  for  an  illus- 
tration of  these  arcs. 

Since  each  such  arc  can  be  given  an  obvious  discrete  characterizing  label,  the 
circular  sequence  of  these  labels  constitutes  a  discrete  representation  of  the 
connected  components  of  FPx{c^)  in  question. 

We  say  that  XqCc^  is  a  critical  placement  if,  when  varying  X  slightly 
near  Xq  some  discontinuous  change  in  the  combinatorial  representation  of 
FPxic^)  takes  place,  namely,  either  the  number  of  components  of  FPx{c^) 
changes  (two  components  merge  into  one  or  vice  versa,  or  a  component 
disappears  or  newly  appears),  or  the  "labeling"  of  one  such  component 
changes. 
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FPj({c2) 


Figure  5  :  Possible  arcs  for  FPx{c^) 


Let  BD*{c^)  =  BD {c^)UBD (r^.Cc^)).  Then  we  have 

Lemma  1:  X^c^  is  a  critical  point  if  and  only  if  the  line  segment  Ai(X)  is 
"tangent"  to  an  element  e^BD*{c^);  more  precisely,  either  an  endpoint  of 
Ai(X)  touches  an  arc  of  BD*(c^),  or  Ai(X)  passes  through  a  vertex  of 
BD  *  (c^),  or  Ai(X)  is  tangent  (in  the  standard  sense)  to  an  arc  of  BD  *  (c^). 

Proof:  Let  XCc^  be  a  point  for  which  Ai(X)  is  tangent  to  an  element 
e  6BD  *  (c  ^ ) ,  in  the  above  sense.  If  c '  =  e  HA  i  (X)  contains  an  infinite  number 
of  points,  then  e'  must  be  a  portion  of  a  radius  emerging  from  Ci,  and  this 
situation  is  easily  seen  to  contradict  the  assumption  on  general  position. 
Hence  we  may  assume  that  e'  contains  only  a  finite  number  of  points.  If  e  is 
an  arc  and  Ai(X)  is  tangent  to  e  in  the  standard  sense,  then  by  property  (P2) 
(at  the  end  of  Section  2),  e'  is  a  singleton,  so  that,  if  we  move  Ai  sUghtly  in 
an  appropriate  direction  to  a  new  placement  Y,  we  can  clearly  achieve  a 
separation  between  e  and  Ai{Y).  The  same  property  is  easily  seen  to  hold  in 
the  two  other  cases,  in  which  either  e  is  a  vertex,  or  the  "tangency"  occurs  at 
an  endpoint  of  Ai(X).  It  follows  that  there  exist  placements  Y  arbitrarily 
close  to  X  at  which  the  combinatorial  representation  of  FPyic^)  is  different 
from  that  of  FPx{c^),  hence  X  is  critical. 

Conversely,  suppose  that  Xic^  is  a  point  for  which  Ai(X)  is  not  tangent 
to  any  element  of  BD*{c^).  Again  by  property  (P2)  and  by  the  assumption 
on  general  position,  each  arc  -y  from  BD*{c^)  either  intersects  SH{Ai{X)) 
transversally  at  a  finite  number  of  points,  or  the  intersection  between  7  and 
SH{Ai{X))  is  empty.  Again  it  is  easily  checked  that  when  Y  varies  suffi- 
ciently near  X,  SH{Ai{Y))  will  continue  to  intersect  7  in  the  same  number  of 
points,  and  be  transversal  to  7  in  each  of  them.  This  is  easily  seen  to  imply 
that  the  combinatorial  representation  of  FPy(c^),  for  Y  sufficiently  near  X,  is 
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the  same  as  that  of  FPx(c^),  hence X  is  non-critical.  □ 

Thus  the  set  of  critical  points  in  c^  is  the  union  of  finitely  many  critical 
curves,  each  containing  placements  X  of  A  i  at  which  A  i  (X)  is  tangent  to  some 
specific  element  from  BD*{c^)  (in  the  sense  of  the  last  lemma).  Each  critical 
curve  is  of  one  of  the  following  types: 

(i)  ^(v): 

The  locus  of  points  X  at  which  Ai(X)  passes  through  some  vertex 
V  CflD  *(c^),  or  is  tangent  (in  the  standard  sense)  at  a  point  v  to  some  arc 
P  of  BD*(c^).  'y(v)  is  a  subsegment  of  a  radius  which  emerges  from  Ci 
and  passes  through  v.  (It  is  easily  checked  that  in  the  case  of  real 
tangency  to  some  arc  p,  there  are  at  most  two  such  points  v  for  each  p.) 

(u)7(X.P): 

The  loai<;  cf  poinds  X  at  which  Ai{X)  touches  some  line  segment 
XiIiD*\^c^)  at  its  endpoint  Pi.  ■v(X,P)  is  the  segment  k  itself  (or  more 
precisely,  it  is  Xflc^). 

im)y(cL,P): 

The  locus  of  points  X  at  which  Ai(X)  touches  some  circular  arc 
aiBD*{c^)  at  its  endpoint  Pi.  7(a,P)  is  the  arc  a  itself  (or  more  pre- 
cisely, it  is  aHc^). 

(iv)^(K,P): 

The  locus  of  points  X  at  which  Ai(X)  touches  some  conchoidal  arc 
K^BD*{c^)  at  its  endpoint  Pi.  7(k,P)  is  the  arc  k  itself  (or  more  pre- 
cisely, it  is  kPIc^). 

(v)-(vii)-y(X,(2),7(a,(2),7(K,G): 

These  are  the  loci  of  points  X  at  which  A  i  (X)  touches  a  line  segment  X ,  a 
circular  arc  a,  or  a  conchoid  k  of  BD*{c^)  at  the  endpoint  Qi.  These 
curves  are  simply  the  images  (within  c^)  of  the  corresponding  arc  X,a, 
or  K  under  the  radial  shift  r^  ^ .  They  are  also  known  as  conchoids,  having 
X,a,  or  K  as  their  base-curves  (see  [Lo]).  We  will  later  analyze  the  alge- 
braic equations  defining  these  conchoids,  and  show  that  they  have  max- 
imum degree  16. 

The  number  of  critical  curves  in  c  Ms  at  most  some  fixed  constant 
(independent  of  c^  and  c^),  and  each  of  them  is  algebraic  of  degree  at  most 
16.  Hence  they  partition  c^  into  at  most  some  fixed  constant  number  K  of 
(relatively  open)  non-critical  regions  (i.e.  connected  components  of  the  com- 
plement of  the  union  of  all  critical  curves).  For  each  such  region  R,  the  com- 
binatorial representation  of  FPx{c^)  clearly  remains  invariant  as  X  varies 
within  R.  (This  holds  even  for  regions  R  that  contain  a  portion  of  a  radial 
segment  e  bounding  c^;  that  is,  for  Xier\R  and  for  points  YiR  approaching 
X  from  within  the  interior  of  c\  the  representations  of  FPxU  )  and  of 
FPyic^)  are  the  same.)  We  denote  this  decomposition  of  c^  by  c^[c^]. 

We  can  now  obtain  a  cell  decomposition  of  PPy  as  follows.  Each  cell  in 
the  decomposition  has  the  form 


-13- 

AiR,L)^{[X,Y]:XiR,  YiML)}, 

where  /?  is  a  non-critical  region  of  c^,  L  is  a  fixed  circular  labeling  of  a 
unique  component  of  FPx(c  ),  that  we  denote  by  <t>x(^)-  We  represent  FPij 
by  a  connectivity  graph  CGij,  whose  nodes  are  these  cells,  and  whose  edges 
represent  adjacency  of  the  ceUs  within  FPij.  The  "crossing  rules"  defining 
these  edges  are  very  similar  to  those  developed  in  earlier  papers  on  the  cell 
decomposition  technique  (see  [SS1],[SS3],[SS4]),  and  we  therefore  omit 
details  of  their  analysis  here.  Roughly  speaking,  these  rules  are: 

(i)    Each  A(/?,L)  is  a  connected  cell, 

(ii)  A(/?,Li)  and  A{R,L2),  for  Li#L2,  are  not  adjacent. 

(iii)  The  cells  A(R' ,L)  and  A(R"  ,L)  (for  the  same  label  L)  are  adjacent, 
where  R' ,  R"  are  non-critical  regions  adjacent  along  a  common  critical 
nrc. 

(iv)  The  cells  A(^',L')  and  A{R  ' ,L")  are  adjacent,  where  R' ,  R"  are  as  in 
(iii),  and  L'  (resp.  L")  is  any  label  associated  with  R'  but  not  with  R" 
(resp.  with  R"  but  not  with  R'). 

To  analyze  the  crossing  rules  between  different  pieces  FPij ,  we  make  use 
of  the  following 

Remark:  Each  connected  component  of  FP{Ai,A2)  is  a  four-dimensional 
manifold.  It  is  well-known  that  such  a  manifold  cannot  be  disconnected  by  a 
submanifold  whose  dimension  is  less  than  three  (see  e.g.  [HW]  p.  48,  and 
[SSI]),  hence  in  developing  the  crossing  rules  between  various  portions  of 
FP{Ai,A2),  it  suffices  to  consider  only  crossings  which  avoid  some  (finite 
number  of)  certain  such  submanifolds. 

Consider  now  any  single  subspace  FPy  within  the  product  cjxcj  from 
our  basic  decomposition  (*)  of  FP{Ai,A2).  The  two-dimensional  cell  cj 
(resp.  cj)  has  two  radial  boundary  edges  which  may  connect  cj  (resp.  cj)  to 
its  (possible)  neighboring  cells  cj-i  and  c}+i  (resp.  cj-i  and  cj+i).  More- 
over, crossing  simultaneously  from  cj  to  a  neighboring  cell  and  from  cj  to  a 
neighboring  cell  amounts  to  crossing  a  two-dimensional  submanifold  of  the 
4-D  space  FP.  By  the  remark  made  above,  the  connectivity  of  FP  does  not 
change  if  we  exclude  from  it  (a  finite  number  of)  such  submanifolds.  Hence 
we  can  ignore  such  simultaneous  crossings,  and  it  suffices  to  analyze  the 
crossing  rules  from  the  four-dimensional  subspace  FPn  to  its  four  neighbor- 
ing subspaces  FP,.ij  ,  FPt  +  ij  ,  FP,j.i  ,  FPtj  +  i.  To  this  end  we  distin- 
guish between  the  following  two  cases:  (1)  crossing  from  FPij  to  the  neigh- 
boring cells  FPi-ij  OT  FP,  +  ij;  (2)  crossing  from  FPtj  to  the  neighboring 
ceUsFP,j_iorFPyj  +  i. 

Consider  first  the  crossing  rule  to  the  subspace  FPt-ij  (FPt  +  ij  is  han- 
dled symmetrically).  Let  R'  and  R"  be.  two  (open)  non-critical  regions  which 
belong  to  the  critical  maps  c}[cj]  and  c}-i[cj],  respectively.  Four  different 
cases  can  arise. 

(i)  R'nR"^0: 
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In  this  case,  this  intersection  must  be  a  subsegment  of  the  radial  segment 
common  to  the  boundaries  of  both  cj  and  cj-i.  For  any  XiR'DR" ,  the 
combinatorial  representation  of  FPxic^)  is  equal  to  the  representation  of 
FPy{c^)  for  any  YiR'DR" .  It  follows  that  there  exists  a  direct  crossing 
between  a  pair  of  4-D  cells  of  the  form  A{R' ,L'),A{R" ,L")  if  and  only 
ifL'=L". 

(ii)  R'r\R"=0,  but  R'  andR"  intersect  each  other  in  a  line  segment  e: 
In  this  case,  e  is  again  a  subsegment  of  the  radial  segment  common  to  the 
two  cells  c},c}-i,  only  this  time  it  is  a  critical  curve  (of  c}[cj]  as  well  as 
of  c/-i[cj]).  It  is  easily  checked  that  in  this  case  the  line  containing  e 
must  also  contain  Ci,  some  vertex  of  c/,  and  some  vertex  of  BD*{cj). 
Since  a  vertex  of  BD*(cj)  is  either  a  vertex  of  cj  or  the  image  of  sucn  a 
vertex  under  the  radial  shift  r^^,  this  situation  contradicts  our  assumption 
on  genera]  position,  so  we  may  ignore  i^ 

(iii)  R'DR"  =0,  but  R'DR"  contains  a  finite  set  of  points: 

Let  X^R'DR".  Since  the  set  {X}xFPx{cj)  is  two-dimensional,  the 
preceding  remark  implies  that  we  can  also  exclude  such  a  crossing  from 
our  rules  without  affecting  the  connectivity  of  FP  (A  1,^2). 

(iv)  R'nR"=0: 

Qearly  in  this  situation  there  does  not  exist  a  direct  free  crossing 
between  any  pair  of  cells  projecting  onto  i?'  and  /?"  respectively. 

Next  we  analyze  the  rule  for  crossing  from  FPij  to  FP{j-i,  where 
FPij  +  i  is  handled  symmetrically.  Let  R'  and  R"  be  two  non-critical  regions 
belonging  to  the  critical  maps  ct[cj]  and  c}[cj-i],  respectively.  We  have  to 
consider  the  following  three  cases. 

(i)  R'nR"^0: 

Suppose  there  is  a  crossing  between  A{R'  ,L')  and  A{R"  ,L")  for  some 
pair  of  labels  V ,  L" .  In  this  crossing  A 2  crosses  from  c?  to  c]-i,  so  its 
tip  must  cross  the  radial  edge  e  common  to  the  boundaries  of  these  two 
cells,  and  thus  both  labels  L' ,  L"  must  include  that  radial  edge.  We 
claim  that  this  is  also  a  sufficient  condition  for  the  crossing  in  question  to 
be  possible.  Indeed  in  this  case,  for  any  point  XiR'C^R" ^  the  two  cells 
4>x(L'),<t)x(L")  are  bounded  by  portions  of  the  common  radial  edge  e, 
and  since  FPxic])  and  FPx{c]-{)  both  intersect  e  in  the  same  connected 
subsegment  (by  property  (PI)  mentioned  in  Section  3),  it  follows  that 
one  can  cross  between  A{R'  ,L')  and  A{R " ,L " )  by  simply  moving  A 1  to 
any  point  X  in  R'DR",  and  then  move  A  2  through  the  segment 
FPx{cj)ne  =  FPx(cj-,)ne. 

(ii)  R'nR"=0,butR'nR"¥^0: 

In  this  case,  the  placements  where  crossing  can  take  place  form  a  2^0 
submanifold  of  FP(Ai,A2)  (because  A 1  then  lies  in  the  1-D  s&X  R'DR" 
and  A 2  hes  on  the  common  radial  edge  e  defined  above).  Hence  by  the 
preceding  remark  we  may  exclude  such  crossings  from  consideration. 
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(iii)  R'nR"=0: 

Qearly  in  this  situation  there  does  not  exist  a  direct  free  passage  between 
any  pair  of  cells  A(/?' ,L'),A(/?"  ,i^")- 
In  summary,  we  have  obtained 

Theorem  1:  (a)  The  combinatorial  complexity  of  FP (A i, A 2)  is  0{n^). 

(b)  A  combinatorial  representation  of  FP(Ai,A2)  in  terms  of  a  discrete  con- 
nectivity graph  CG{Ai,A2)  can  be  constructed  in  0{n^)  time  and  space, 
using  the  above  cell  decomposition  technique. 

(c)  Any  motion  planning  instance  for  Ai,  A 2  can  then  be  solved  in  time 
0{n^)  by  path  searching  in  CG(Ai,A2). 

Remarks:  (1)  Concerning  the  issue  of  lower  bounds  for  our  motion  coordina- 
tion problem,  we  note  that  one  can  not  improve  the  upper  bound  in  Theorem 
1  by  considering  orJy  a  Ingle  comiecled  component  of  FP{Ai,A2)  (as  dis- 
cussed in  Section  2),  because  (as  illustrated  in  Fig.  6)  a  connected  component 
of  fP  (A  1,^2)  can  have  n(n^)  vertices. 


Ci 
A] 


n/2 

Wall  Edges 


n/2  Wall  Edges 


Figure  6:  A  component  of  FP(Ai,A2)  having  Cl(n^)  complexity 


Furthermore,  in  this  example  each  cell  cj  in  FP(Ai),  l<i<ni,  intersects 
each  cell  c J  in  FP (A 2),  l^j<n2  (where  both  rii  and  n2  are  0(n));  hence  the 


number 


of 


cells 


cj  x'c^ 


y' 


l<j</ii,  l<y<n2,  for  which  c}ncj=^0  (i.e.  in 


which  the  two  arms  Ai  and  A  2  can  interfere  with  one  another  within  cjxcj) 
is  n(n^).  However,  this  example  does  not  provide  any  lower  bound  for  an 
algorithm  to  coordinate  the  free  motion  of  two  planar  Stanford  arms.  We 
note  that  it  is  an  open  problem  whether  one  can  coordinate  the  free  motion 
of  two  independent  robot  systems  Bi  and  B2  in  time  which  is  substantially 
less  than  0{nin2),  where  n^  and  n2  are  the  times  needed  to  separately  plan 
the  free  motion  of  5i  and  of  52.  respectively  (each  moving  in  the  presence  of 
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static  obstacles  only). 

(2)  Fortune,  Wilfong  and  Yap  [FWY]  have  also  presented  an  0{nHogn) 
algorithm  for  coordinating  the  motion  of  two  dependent  planar  Stanford 
arms,  in  which  the  two  respective  tips  of  the  arms  are  constrained  to  remain 
in  contact  with  each  other  as  they  move  (namely,  to  be  co-located).  The  free 
configuration  space  for  this  case  is  denoted  by  FP (A  1M2),  and  can  be 
represented  simply  as  the  intersection  FP(Ai)nFP(A2)  (where,  as  above, 
both  spaces  are  represented  by  the  position  of  the  common  tip).  The  algo- 
rithm in  [FWY]  calculates  this  intersection  by  using  a  standard  plane  sweep, 
which  takes  time  0{{n+p)\og  n)  where  p  is  the  actual  number  of  intersec- 
tions between  the  boundaries  of  FP(Ai)  and  FP  (A 2)  (p  is  n(/i^)  in  the  worst 
case).  We  note  here  that  by  using  the  above  angular  decompositions  of 
FP{Ai)  and  FP^Ai),  we  can  compute  FP(Ai*A2)  in  time  0(n^)  only. 
Indeed,  note  that 


"1    "2 


FPiAi)nFPiA2)=    U    U    iclDtj)  , 


n/2 


^—    Wall  Edges 


n/2  Wall  Edges 


Figure  1:  FP{Ai*A2)  has  n(n^)  complexity 


and  for  each  pair  of  indices  (/,;),  l</<ni,  l<j<n2,  we  can  compute  cjOcj 
(and  glue  it  to  its  appropriate  neighbors)  in  constant  time.  This  shows  that 
the  overall  complexity  of  FP  (A  i*A2)  is  0{n-),  and  that  it  can  be  computed 
in  O(n^)  time.  Moreover,  an  easy  example  (illustrated  in  Fig.  7)  shows  that 
in  the  worst  case  the  complexity  of  FP{Ai*A2)  can  be  Cl{n^),  thus  the  above 
algorithm  is  optimal  in  the  worst  case,  //  the  entire  space  ^^(^1*^2)  is  to  be 
constructed. 

However,  we  can  actually  do  much  better.  Indeed,  as  has  been  observed  e.g. 
in  [PSS],[GSS]  (see  also  the  discussion  in  section  2),  it  suffices  to  calculate 
only   the   connected   component   C   01    .P{Ai*A2)    containing   the   initial 
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configuxation  Z  of  the  two  arms,  since  no  other  part  of  FP(Ai*A2)  can  be 
reached  from  that  configuration  by  a  free  motion.  But  C  is  simply  the  con- 
nected component  of  the  intersection  FP(Ai)nFP(A2)  which  contains  Z.  A 
"combination  lemma"  presented  in  [GSS]  implies  that  the  combinatorial  com- 
plexity of  C  is  linear  in  the  combinatorial  complexity  of  FP(Ai),  FPiAi), 
and  is  thus  0(n).  Moreover,  the  "red-blue  merge"  procedure  of  [GSS]  can  be 
used  to  calculate  C  in  time  0{n  log  n).  We  thus  obtain 

Theorem  2:  One  can  plan  the  coordinated  motion  of  two  planar  Stanford 
arms,  where  the  tips  of  the  two  arms  are  constrained  to  touch  one  another,  in 
time  (?(n log  n). 

5.  Coordinating  the  motion  of  two  independent  discs  (or  translating  convex 
objects)  in  the  plane 

Next  we  consider  the  sncdal  case  iri  wliich  the  ^vo  L'dcpendently  rr-o\ing 
robots  are  two  discs  Bi  and  Bi  having  radii  ri  and  ri,  respectively,  which  are 
free  to  move  in  a  2-D  polygonal  region  as  in  the  previous  sections.  As  noted 
in  the  introduction,  this  problem  has  been  studied  in  [SS3]  who  obtain  an 
0{n^)  algorithm,  using  their  cell  decomposition  technique,  and  in  [Yal]  who 
gives  an  improved  0{n^)  algorithm,  based  on  a  certain  retraction  of  FP  onto 
its  boundary  BFP.  Recently  [RA]  have  also  proposed  a  new  O(n^)  algorithm, 
but  the  details  of  their  algorithm  are  somewhat  imclear.  Here  we  show  that 
this  problem  can  be  handled  in  much  the  same  way  as  the  case  of  two  Stan- 
ford arms,  and  obtain  an  O(n^)  algorithm  which,  although  does  not  improve 
the  result  of  Yap  [Yal] ,  puts  this  problem  in  the  more  general  framework  of 
coordinated  motion  planning  that  we  have  developed.  Also  our  algorithm 
can  be  extended  to  handle  coordinated  translational  motion  of  any  pair  of 
independent  fairly  arbitrary  convex  objects  in  the  plane,  provided  the  com- 
plexity of  each  of  them  is  small  and  fixed. 

In  the  case  of  discs,  the  space  FP{Bi)  (and  similarly  FPiBi))  is  obtained 
by  "growing"  each  obstacle  by  the  radius  of  the  disc  B-i,  and  forming  the 
complement  of  the  union  of  the  expanded  obstacles.  It  follows  e.g.  from  the 
analysis  of  [KLPS]  that  the  boundary  of  each  of  these  spaces  is  composed  of 
0(ji)  line  segments  and  circular  arcs,  and  that  these  two  spaces  can  be  calcu- 
lated in  time  O(nlog^n)  (as  in  [KLPS]),  or  by  an  improved  C>(nlog  n)  algo- 
rithm based  on  Voronoi  diagrams  (see  [OY]  and  [Ya2]).  It  is  easily  seen  that 
FP{Bi)  (resp.  FPiBi))  can  be  "vertically  decomposed"  into  ni=0(n)  (resp. 
n2  =  o{n))  simple  cells  c\,  .  .  .  ,c\^  (resp.  c\,  .  .  .  ,cl^)  such  that  the  boun- 
dary of  each  cell  c]  (resp.  c])  consists  of  a  "floor"  and  a  "ceiling"  (which  are 
line  segments  or  circular  arcs  of  radius  rj  (resp.  ri)  bounding  some  expanded 
obstacles)  and  of  two  straight  vertical  edges  (one  of  which  may  degenerate  to 
a  single  point)  which  connect  c}  (resp.  c))  to  its  neighboring  cells  (see  Fig. 
8).  Assuming  general  position  of  the  obstacles,  it  is  easily  seen  that  each  of 
these  cells  can  have  at  most  four  neighboring  cells. 
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B, 


Figure  8:  The  vertical  decomposition  of  FP  (Bi) 


As  in  Section  4  we  can  decompose  FP  (81,82)  as 

(**)  U    U  FP(5i,B2)I(WxcJ)    . 

/=iy=i 

In  each  subspace  FP/y =FP (fl  1,^2) I (cjxc;).  1^/^ni,  1^j^n2,  we  can  apply 
a  cell  decomposition  technique  similar  to  that  of  Section  4,  where  (in  the 
notations  introduced  above)  for  each  X^cj  the  set  FPx{cj)  is  the  set  differ- 
ence of  cj  and  a  closed  circle  centered  at  X  and  having  radius  ri  +  r2.  The 
critical  curves  that  arise  in  cj  are  simpler  than  those  obtained  in  the  case  of 
Stanford  arms  ~  they  are  all  circular  arcs  and  straight  segments  (see  [SS3]  for 
more  detail).  Thus  one  can  obtain  (in  constant  time)  an  appropriate  discrete 
connectivity  graph  CGij  which  represents  FPij.  The  gluing  procedure  of  the 
nin2  graphs  CGij  into  an  overall  connectivity  graph  €0(81,82)  can  be  done 
in  a  similar  manner  to  the  gluing  procedure  in  Section  4,  in  constant  time  per 
each  pair  of  adjacent  cells.  We  omit  the  technical  details  of  developing  the 
relevant  crossing  rules  (which  are  similar  in  spirit  to  those  obtained  in  Section 
4). 

We  also  note  that  the  analysis  presented  above  can  be  easily  generalized 
to  the  case  in  which  81,  82  are  convex  objects  translating  in  the  plane,  pro- 
vided that  the  complexity  of  81  and  of  5 2  is  small  and  fixed  (e.g.  they  are 
both  convex  polygons  with  a  small  number  of  edges).  The  results  of  [lOLPS] 
imply  that  the  complexity  oi  FP  (8 {) ,  FP (8 2)  is  still  0(n),  and  all  the  other 
steps  of  the  algorithm  can  be  similarly  generalized.  (Note  that  we  require 
that  81  and  82  have  small  fixed  complexity  to  ensure  that  each  of  the  spaces 
FPij  involves  only  0(1)  geometric  constraints  and  thus  has  constant  complex- 
ity and  can  be  calculated  in  constant  time.)  Omitting  details  of  the  fairly 
straightforward  extension  of  the  algorithm,  we  conclude 
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Theorem  3:  Gx)rdinated  motion  planning  for  two  independent  discs  (or 
translating  convex  objects  of  small  fixed  complexity)  in  a  2-D  polygonal 
region  bounded  by  n  edges,  can  be  accomplished  in  O(n^)  time. 

6.  Conclusion 

The  new  approach  to  coordinated  motion  planning,  as  presented  in  this 
paper,  can  be  applied  to  many  other  cases  of  two  independently  moving  robot 
systems  B  i  and  B  i ,  yielding  a  simple  and  general  algorithm  for  calculating 
FPiBi.Bi),  whose  complexity  is  proportional  to  the  product  of  the  combina- 
torial complexities  of  FP{Bi)  and  of  FPiBi)  (or  of  their  relevant  connected 
components).  The  main  condition  which  is  required  for  our  technique  to 
apply,  is  the  capability  to  decompose  FP{Bi)  and  FP{Bi)  into  collections  of 
ceUs,  each  having  a  simple  shape  with  0(1)  complexity,  so  that,  within  each 
Cartesiryi  pio-'Iuc*  of  two  such  cells,  FP{B^^,B^)  has  unly  0(1)  combinatorial 
complexity.  If  FP{Bi)J^P{Bi)  are  algebraic,  then  such  decompositions  gen- 
erally exist  (e.g.  the  Collins  decomposition  [SS2]  has  the  required  property). 
However,  the  number  of  cells  needed  to  form  such  a  decomposition  (let  us 
call  it  a  "triangulation")  may  generally  be  much  larger  than  the  "actual"  com- 
binatorial complexity  of  FP  (as  measured  e.g.  by  the  complexity  of  its  boun- 
dary). For  2-D  spaces,  Euler's  formula  generally  makes  such  triangulations  of 
small  size  easy  to  obtain,  but  it  appears  to  be  an  open  problem  whether  a 
three-dimensional  algebraic  variety  can  be  triangulated  into  a  number  of  cells 
proportional  (or  closely  related)  to  its  combinatorial  complexity. 

We  also  note  that  even  when  FP  can  be  triangulated  into  a  small  number 
of  simple  cells,  ir  might  be  the  case  that  the  complexity  of  FPij  (within  a  sin- 
gle product  cell  c/  xcy)  is  not  constant.  This  may  be  the  case  when  the  mov- 
ing systems  fli,B2  have  non-constant  complexity.  For  example,  if  Bi  and  Bi 
are  two  simple  polygons  having  k^  and  ki  vertices,  respectively,  then  the 
complexity  of  FPy  will  generally  be  a  polynomial  (of  rather  high  degree)  in 
ki  and  ki,  so  our  approach  does  not  yield  as  efficient  an  algorithm  as  those 
presented  above.  The  difficulty  can  sometimes  be  overcome  using  ad-hoc 
techniques.  For  example,  in  the  case  of  two  polygons,  we  could  have  decom- 
posed each  of  the  polygons  into  0{k{)  and  Oiki)  triangles  respectively,  solve 
the  problem  for  each  pair  of  such  triangles  (one  from  each  polygon),  and 
finally  intersect  the  resulting  spaces  (as  in  the  case  of  dependent  Stanford 
arms)  to  obtain  the  desired  FP. 

As  already  mentioned  earlier,  we  note  that  our  jqjproach  is  quite  flexible 
in  the  manner  in  which  FPij  is  to  be  calculated  within  each  product  cell 
c}y.cj.  One  can  use  the  Collins  cell  decomposition  technique  [SS2],  the  alge- 
braic roadmap  technique  of  Canny  [Ca],  or  any  other  specific  convenient 
technique  which  yields  reasonable  discrete  structures  that  represent  each  FPij . 
The  gluing  of  these  structures  to  one  another  can  also  be  accomplished  by  any 
convenient  method  (e.g.  by  calculating  the  restriction  of  FP  to  c}  x  (cy  Uc)*i) 
or  (c/Uc/*i)xcj  (in  constant  time)  and  deducing  from  these  structures  the 
adjacency  relationships  across  cells).  However,  our  technique  can  be  applied 
only  if  each  of  the  "underlying"  spaces  FP{B{),FP{B2)  can  be  decomposed 
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into  a  small  number  of  simple  cells,  so  only  cell  decomposition  techniques 
appear  to  be  adequate  for  the  initial  calculation  of  these  spaces. 

As  an  example  where  all  these  conditions  are  satisfied,  we  can  extend 
our  technique  to  obtain  an  0{n^)  coordinated  motion  planning  algorithm  for 
two  independent  line  segments  ("rods")  translating  and  rotating  in  a  2-D 
polygonal  space  with  n  coraers,  because  each  of  the  spaces  FP(B/)  can  be 
represented  by  a  cell  decomposition  of  complexity  0{n^)  (by  the  analysis  in 
[LS]),  and  the  complexity  of  each  rod  is  small  and  fixed. 

Similarly,  a  recent  algorithm  in  [AS]  gives  a  cell  decomposition  of  a  sin- 
gle component  of  the  free  space  of  an  arbitrary  polyhedron  (of  0(1)  com- 
plexity) translating  in  3-space  amidst  polyhedral  obstacles  having  n  faces  alto- 
gether. This  decomposition  has  at  most  0{n'"^'*'*)  cells,  for  any  €>0.  Hence, 
using  this  algorithm,  we  can  obtain  an  0(n^'*'^'*"*)  algorithm  for  coordinated 
motion  planning  of  two  iudependeiit  translating  poloyhcdia. 

In  contrast,  several  efficient  algorithms  have  been  recently  obtained  for 
various  moving  systems,  which  produce  different  representations  of  the  free 
space  FP,  and  do  not  seem  to  yield  comparably  efficient  cell  decompositions. 
A  typical  example  is  the  algorithm  in  [KS]  for  a  (translating  and  rotating) 
convex  polygon  in  a  2-D  polygonal  space.  Such  algorithms  do  not  immedi- 
ately generalize  under  our  technique  to  efficient  coordinated  motion  planning 
algorithms  for  pairs  of  such  systems. 

Another  important  practical  observation  is  that  in  certain  cases  it  is  possi- 
ble for  many  subspaces  FPij  to  coincide  with  the  corresponding  product  cells 
cjx-cj.  For  example,  in  the  case  of  two  independent  discs,  if  the  radii  of  the 
discs  are  much  smaller  than  the  dimensions  of  the  obstacles,  any  subset  FPij, 
for  which  the  two  discs  lie  in  different  (and  relatively  distant)  portions  of  the 
workspace,  will  coincide  with  cj  x  cj .  This  observation  implies  that  there  is 
no  need  to  calculate  these  FP,/s,  or  even  to  store  them  expUcitly.  Although 
we  have  not  explored  this  observation  in  full,  we  regard  it  as  a  potentially 
significant  source  for  further  optimization  of  our  algorithm.  Of  course,  if  the 
two  systems  tend  to  be  generally  near  one  another,  so  that  interference 
between  them  is  likely  to  occur  in  many  product  cells,  then  this  optimization 
will  fail  to  be  significant  (see  Fig.  6  for  example). 

Next  we  make  the  following  general  observation  concerning  coordinated 
motion  planning.  If  the  two  moving  systems  have  respectively  ki,k2  degrees 
of  freedom,  then  by  the  general  recent  results  of  Canny  [Ca],  coordinated 
motion  planning  for  this  pair  of  systems  (which  has  ki  +  k2  degrees  of  free- 
dom) can  be  accompUsheid  in  time  close  to  C>(n*^"*'*^)  (again,  ignoring  the 
algebraic  complexity  of  the  constraints).  However,  whenever  the  motion 
planning  problem  for  each  of  the  systems  separately  can  be  solved  in  better 
than  0{n  ')  or  0(n*')  time  (by  a  cell  decomposition  technique),  our  tech- 
nique yields  an  improved  algorithm  for  the  coordinated  problem  (e.g.  in  the 
case  of  two  independent  rods  mentioned  above,  our  0{n^)  algorithm 
improves  the  0{n^\ogn)  algorithm  yielded  by  Canny's  procedure). 

Finally,  a  natural  generalization  of  our  approach  is  to  apply  it  to  k 
independent  robot  systems  Bi,  .  .  .  ,5^.   It  seems  that  our  approach  can 
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potentially  yield  a  decomposition  of  FP(fli,  .  .  .  ,Bt)  into  "local"  subspaces 
which  can  be  properly  glued  to  one  another  to  form  the  entire  space,  so  that 

the  total  time  and  space  needed  to  obtain  this  representation  is  0(C(k)  Tl  n/), 

where  «/  is  the  combinatorial  complexity  of  a  cell  decomposition  of  FP(Bi), 
l^i^k  (or  of  some  specific  connected  component  thereof),  and  C{k)  is  an 
upper  bound  for  the  time  needed  to  calculate  FP(Bi,  .  .  .  ,Bt)\(c^x  •  •  •  xc*) 
(where  each  c'  is  a  cell  in  the  decomposition  of  FP{Bt),  l<i<it).  In  particu- 
lar, if  Bi,  .  .  .  ,Bjfc  are  it  planar  discs  (or  k  planar  Stanford  arms),  then  our 
technique  would  yield  an  0{C(k)n^)  algorithm  for  calculating 
FP(Bi,  .  .  .  ,5t),  thus  reproducing,  and  generalizing,  the  results  of  [RA]. 
The  constant  C(k)  will  in  general  be  quite  large.  The  recent  results  of  [Ca] 
suggest  that  it  might  be  only  singly  exponential  in  k. 
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Appendix  A  :  Analysis  of  conchoids  and  calculation  of  critical  points 

In  this  appendix  we  present  a  few  technical  details  pertaining  to  the  alge- 
braic structure  of  the  critical  curves  arising  in  the  case  of  two  Stanford  arms. 
These  curves  are  line  segments,  circular  arcs  or  conchoidal  arcs,  and  we  con- 
sider here  only  the  conchoids.  Let  -y  be  some  simple  (algebraic)  curve  in  the 
plane,  and  let  B  =PQ  be  a  line  segment  which  moves  in  the  plane  such  that 
the  endpoint  Q  touches  the  curve  -y  and  B  is  constrained  to  touch  a  fixed 
point  C.  Then  as  noted  in  Section  4,  the  other  endpoint  P  of  B  moves  along  a 
conchoid  k('y,C)  (having  -y  as  a  base-curve  and  C  as  a  fixed-point),  "y^^thout 
loss  of  generaUty  assume  that  C  is  the  origin  of  our  coordinate  system.  Let 
P  =  (x,y)  and  Q={x',y').  U  d  denotes  the  length  of  PQ,  then  it  is  easily 
chedced  that  (x,y)  and  {x' ,y')  are  related  by  (see  also  Fig.  9) 
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P(x.y) 


Figure  9:  A  conchoid 


X'  =  x(l 


If  7  is  the  straight  line  y'  =  ax'+b,  then  k(7,C)  is  a  conchoid  of  Nicomedes 
(see  also  [SSI])  whose  equation  is  thus 

ix^+y^)(y -ax-by  =  d^(y-ax)^    . 

It  is  easily  verified  that  if  7  is  a  quadric,  then  the  equation  defining  the  con- 
choid k(7,C)  is  algebraic  of  degree  8;  and  in  general  if  7  is  algebraic  of 
degree  k,  then  k('y,C)  is  algebraic  of  degree  (at  most)  4k.  In  particular,  if  -y 
is  a  conchoid  of  Nicomedes  (i.e.  y  is  of  the  fourth  degree),  then  k(7,C)  is 
algebraic  of  degree  16. 

Thus  the  degree  of  any  of  our  critical  curves  is  at  most  16.  Since  the 
computation  of  the  decomposition  of  cj  into  non-critical  regions  involves 
intersections  of  pairs  of  critical  curves,  we  may  face  (repeatedly)  the  task  of 
intersecting  pairs  of  algebraic  curves  of  fairly  large  degree,  which  can  be 
quite  cumbersome.  Nevertheless,  we  have 

Lemma  A.l:  Any  intersection  between  a  pair  of  critical  curves  in  cj  can  be 
obtained  by  solving  a  pair  of  algebraic  equations  one  of  which  is  of  degree  at 
most  4. 

Proof:  K  the  degree  of  both  curves  to  be  intersected  is  greater  than  4,  then 
each  of  them  must  be  of  the  form  7 (a, 2)  or  y{K,Q).  But  then  we  can 
express  the  desired  intersection  in  terms  of  the  position  of  the  endpoint  Q 1 , 
which  is  plainly  obtained  by  solving  a  pair  of  equations,  each  of  degree  at 
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most  4.  n 

The  proof  of  Lemma  A.l  is  easily  seen  to  imply  that  the  most  difficult 
calculation  that  we  may  encounter  is  of  the  intersection  between  a  conchoid 
of  Nicomedes  and  an  algebraic  curve  P{x,y)=0  of  degree  at  most  16.  Actu- 
ally, using  a  more  careful  analysis,  we  can  show  that  the  intersection  of  two 
critical  curves  can  always  be  obtained  by  solving  a  pair  of  equations,  such 
that  either  both  are  of  degree  at  most  four,  or  one  of  them  is  quadratic  (and 
the  other  is  of  degree  at  most  16). 
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